/**
 * @file app.c
 * @author Wu TongXing (Lucas@hiwonder.com)
 * @brief 主应用逻辑
 * @version 0.1
 * @date 2023-05-08
 *
 * @copyright Copyright (c) 2023
 *
 */

#include "cmsis_os2.h"
#include "led.h"
#include "lwmem_porting.h"
#include "global.h"
#include "lvgl.h"
#include "lv_port_disp.h"
#include "global.h"
#include "adc.h"
#include "u8g2_porting.h"
#include "pwm_servo.h"
#include "serial_servo.h"
#include "iic_sensor_porting.h"

/* 云台舵机限位参数 */
#define  HOLDER_MIN   500  	//对应舵机的0°
#define  HOLDER_MAX  2500	//对应舵机的180°

#define PWM_SERVO_DEV_MAX  2450
#define PWM_SERVO_DEV_MIN  550

/* 蜂鸣器	的频率 */
#define BUZZER_FREQ				 1300

/* 硬件初始化声明 */
void buzzers_init ( void ); //蜂鸣器
void buttons_init ( void );	//按键
void leds_init ( void );		//LED
void motors_init ( void );  //电机
void pwm_servos_init ( void ); //舵机
void chassis_init ( void ); //小车参数初始化

//按键回调函数
void button_event_callback ( ButtonObjectTypeDef *button,  ButtonEventIDEnum event )
{
    if ( button == buttons[0] ) { /* 按键1的事件  */
        if ( event == BUTTON_EVENT_CLICK ) {	//若为单击
             //进入中断级代码临界区
//            uint32_t ret = taskENTER_CRITICAL_FROM_ISR();
//            //写入Flash
//            hw_flash_write ( SAVE_ADD,&Chassis_run_type,1 );
//            //退出中断级代码临界区
//            taskEXIT_CRITICAL_FROM_ISR ( ret );
            //鸣响
            buzzer_didi ( buzzers[0], 1800, 100, 200, 1 );	//蜂鸣器响，提示底盘类型
        }
    }

    if ( button == buttons[1] ) { /* 按键2的事件  */
        if ( event == BUTTON_EVENT_LONGPRESS ) {	//若为长按
				}

        if ( event == BUTTON_EVENT_CLICK ) {	//若为长按

        }
    }
}

//IMU读取定时器回调函数
void IMU_read_timer_callback ( void *argument )
{
    //注意：单片机尽量不要打印浮点数
    static float rpy[3] = {0,0,0};
    static char msg[16];
    imus[0]->get_euler ( imus[0], rpy );	//读取IMU的数据
    sprintf ( msg, "Roll :%3.2f", rpy[0] );	//将数据转化为字符串
    printf ( "%s\n",msg );		//打印字符串
    sprintf ( msg, "Pitch:%3.2f", rpy[1] );	//将数据转化为字符串
    printf ( "%s\n",msg );		//打印字符串
    sprintf ( msg, "Yaw  :%3.2f", rpy[2] );	//将数据转化为字符串
    printf ( "%s\n",msg );		//打印字符串
}

//小阿克曼车控制函数
void minacker_control ( void );
// 循迹测试函数
void trace_test ( void ); //循迹测试
void servo_motor_test( void ); //伺服转向电机测试
void classis_motor_test( void );//地盘电机测试

/* 用户入口函数 */
void app_task_entry ( void *argument )
{
    /* 声明外部句柄 */
    //蜂鸣器句柄
    extern osTimerId_t buzzer_timerHandle;
    //电量监控句柄
    extern osTimerId_t battery_check_timerHandle;
    //LED控制句柄
    extern osTimerId_t led_timerHandle;
    //按键控制句柄
    extern osTimerId_t button_timerHandle;
    //IMU定时器句柄
    extern osTimerId_t IMU_read_timerHandle;

    //运动控制的队列句柄
    //手柄的信号是在 gampad_handle.c 中USBH_HID_EventCallback()回调函数中压入
    //在用户入口函数中取出控制小车运动
//	extern osMessageQueueId_t moving_ctrl_queueHandle;  // todo：删掉 用不着

    /* 硬件初始化 */
    motors_init();      //电机初始化
    pwm_servos_init();  //云台舵机初始化
    leds_init();		//LED初始化
    buzzers_init();     //蜂鸣器初始化
    buttons_init();		//按键初始化

    //注册按键回调函数，处理按键值
    button_register_callback ( buttons[0], button_event_callback );
    button_register_callback ( buttons[1], button_event_callback );

    //开启LED定时器
    osTimerStart ( led_timerHandle, LED_TASK_PERIOD );
    //开启蜂鸣器定时器，让其在中断中运作，后面调用接口函数即可
    //参数1：定时器句柄 ， 参数2：定时器的工作间隔 ms
    osTimerStart ( buzzer_timerHandle, BUZZER_TASK_PERIOD );
    //开启按键定时器
    osTimerStart ( button_timerHandle, BUTTON_TASK_PERIOD );
    //开启电量监控定时器，实时监控电量
    osTimerStart ( battery_check_timerHandle, BATTERY_TASK_PERIOD );
    //开启IMU读取定时器，500ms读取一次
    osTimerStart ( IMU_read_timerHandle, IMU_TASK_PERIOD );

    HAL_ADC_Start ( &hadc1 );

//    char msg = '\0';  // todo：删掉 用不着
//    uint8_t msg_prio;  // todo：删掉 用不着
    //初始化 运动控制的队列（参数：队列句柄）
//    osMessageQueueReset ( moving_ctrl_queueHandle );   // todo：删掉 用不着

    //初始化底盘电机运动参数
    chassis_init();
    //选择底盘类型  注意：阿克曼车底盘 使用 CHASSIS_TYPE_JETACKER  ! ! !
    set_chassis_type ( CHASSIS_TYPE_JETACKER );
		chassis->stop ( chassis ); // 初始化后  停止
		buzzer_didi ( buzzers[0], 1800, 100, 200, 1 );	//蜂鸣器响

    printf("trace start.\n");
		
    //trace_test();    //伺服转向电机测试  内部无循环
		//servo_motor_test();    //伺服转向电机测试  内部无循环
		classis_motor_test();  //地盘电机测试  内部无循环
		
    // 循环  : RTOS任务中的循环，必须要有osDelay或者其他系统阻塞函数，否则会导致系统异常
    for ( ;; ) {
				printf("iic trace sensor = %x\n",read_iic_secsor ());
				osDelay ( 1000 ); //延时5s
    }
}

/*
*  阿克曼控制函数
*  参数：控制命令
*  该函数通过接收char类型的参数，来判断运行哪一个动作
*/
void jetacker_control ( char msg )
{
    //定义电机运动速度
    //建议范围为 [50 , 300]
    static float speed = 50.0f;

    //以x轴 speed 的线速度 运动（即向前运动）
    chassis->set_velocity ( chassis, speed, 0, 0 );
    osDelay ( 2000 ); //延时2s
    buzzer_didi ( buzzers[0], 1800, 100, 200, 3 );

    chassis->stop ( chassis ); //停止
    osDelay ( 5000 ); //延时1s
    buzzer_didi ( buzzers[0], 1800, 100, 200, 3 );

    chassis->set_velocity ( chassis, speed, 0, 0 ); //向后运动
    osDelay ( 2000 ); //延时2s

    chassis->stop ( chassis ); //停止
    osDelay ( 5000 ); //延时1s
    buzzer_didi ( buzzers[0], 1800, 100, 200, 4 );

    //以x轴 speed 的线速度，绕自身左边的半径500mm圆做转圈运动
    chassis->set_velocity_radius ( chassis, speed, 500, true );
    osDelay ( 2000 ); //延时2s

    chassis->stop ( chassis ); //停止
    osDelay ( 5000 ); //延时1s

    //以x轴 speed 的线速度，绕自身左边的半径500mm圆做转圈运动
    chassis->set_velocity_radius ( chassis, -speed, 500, true );
    osDelay ( 2000 ); //延时2s

    chassis->stop ( chassis ); //停止
    osDelay ( 5000 ); //延时1s

    chassis->set_velocity_radius ( chassis, speed, -500, true );
    osDelay ( 2000 ); //延时2s

    chassis->stop ( chassis ); //停止
    osDelay ( 5000 ); //延时1s

    chassis->set_velocity_radius ( chassis, -speed, -500, true );
    osDelay ( 2000 ); //延时2s

    chassis->stop ( chassis ); //停止
    osDelay ( 5000 ); //延时1s
}
/*
*  循迹测试函数 
*/
void trace_test ( void )
{
//    static float speed = 50.0f;  
//		uint16_t cnt = 0;
	
		//pwm_servo_set_position ( pwm_servos[0] , 2000 , 200 );
		chassis->set_velocity_radius ( chassis, 20, 220, true );
		osDelay ( 6000 );
	
		//chassis->set_velocity_radius ( chassis, 0, 0, true );
		 chassis->stop ( chassis ); //停止
		osDelay ( 200 );
		pwm_servo_set_position ( pwm_servos[0] , 1500 , 200 );
		osDelay ( 2000 );
		buzzer_didi ( buzzers[0], 1800, 100, 200, 2 );	
	
//    buzzer_didi ( buzzers[0], 1800, 300, 300, 1 );	//开始提示
//    osDelay ( 5000 ); //延时6s
//    buzzer_didi ( buzzers[0], 1800, 300, 300, 2 );	//延时结束提示

//    //以x轴 speed 的线速度 运动（即向前运动）
//    chassis->set_velocity ( chassis, speed, 0, 0 );
//		
//		while(cnt < 200)
//		{
//				cnt++;
//			 if(read_iic_secsor () != 0)
//			 {
//				 break;
//			 }
//			 osDelay ( 50 ); //延时6s
//		}
//    osDelay ( 10000 ); //延时6s
    chassis->stop ( chassis ); //停止
    buzzer_didi ( buzzers[0], 1000, 600, 30, 1 );	//停止运行提示

}

/*
*  伺服转向电机测试函数
*   函数内部无循环
*/
void servo_motor_test( void )       //伺服转向电机测试
{
        pwm_servo_set_position ( pwm_servos[0] , 2100 , 500 );
        osDelay ( 2000 );
        pwm_servo_set_position ( pwm_servos[0] , 1000 , 500 );
        osDelay ( 2000 );
}
/*
*  地盘电机测试函数
*   函数内部无循环
*/
void classis_motor_test( void )     //地盘电机测试
{
		pwm_servo_set_position ( pwm_servos[0] , 2000 , 120 );
		osDelay(300);
		pwm_servo_set_position ( pwm_servos[0] , 800 , 120 );
		osDelay(300);
		pwm_servo_set_position ( pwm_servos[0] , 2000 , 120 );
		osDelay(300);
		chassis->set_velocity ( chassis, 100.0, 0, 0 );
		osDelay(2000);
		chassis->stop ( chassis );
		osDelay(1000);
		
//		chassis->set_velocity_radius ( chassis, 300.0,625, true );
//		osDelay(1500);
//		chassis->stop ( chassis );
//		osDelay(1000);		
}



